/***************************************************************************
 创建者: 华磊
 开始时间: 2019.5.4
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写
 ***************************************************************************/
#include "couplecontrolmanagement.h"
#include "motionmessage.h"
#include "axisgroup.h"
#include "filteringalgorithm.h"

CoupleControlManagement::CoupleControlManagement(QString configFileIn, MotionMessage *motionMessageIn,
                                                 int robotIdIn,AxisGroup *axisGroupIn)
{
    robotId=robotIdIn;
    motionMessage=motionMessageIn;
    axisGroup=axisGroupIn;
    isInitialOkStatus=false;
    isUseCoupleControl=false;

    filtering_mapFollowAxisPos=new FilteringAlgorithm;

    if(1==initialConfigFile(configFileIn))
    {
        isInitialOkStatus=true;

    }
    else
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoupleControlManagement", 22150, robotId);
    }
}

bool CoupleControlManagement::isRelativeDistanceApproachTrigged()
{
    for(int i=0;i<coupleParameterList.size();i++)
    {
        if(coupleParameterList[i].isNeedCease)
        {
            return true;
        }

    }
    return false;
}

int CoupleControlManagement::setCoupleRelativeDistanceMonitorEnable(int followAxisId, bool isEnableIn)
{
    for(int i=0;i<coupleParameterList.size();i++)
    {
        if(followAxisId==coupleParameterList[i].followAxisId)
        {
            resetCoupleOffset(coupleParameterList[i],axisGroup);
            coupleParameterList[i].isMonitorRelativeDistance=isEnableIn;
            coupleParameterList[i].isNeedCease=false;
            //usingMutex.unlock();
//            qDebug()<<" setCoupleControlEnable sucess";
            return 1;
        }

    }
    //usingMutex.unlock();
//    qDebug()<<" setCoupleControlEnable failed, no exist couple axis";
    return -1;
}

int CoupleControlManagement::getCoupleControlInfo(std::vector<CoupleConfigParameter> &infoOut)
{
    //usingMutex.lock();
    infoOut=coupleParameterList.toStdVector();
    //usingMutex.unlock();
    return 1;
}

bool CoupleControlManagement::isCoupleControlFollowAxisWork(int followAxisId)
{
    //usingMutex.lock();
    for(int i=0;i<coupleParameterList.size();i++)
    {
        if(followAxisId==coupleParameterList[i].followAxisId)
        {
            if(coupleParameterList[i].isValid)
            {
                //usingMutex.unlock();
                return true;
            }
        }
    }
    //usingMutex.unlock();
    return false;
}

int CoupleControlManagement::getCoupleConfigInfo(int followAxisId, CoupleConfigParameter &coupleParaOut)
{
    //usingMutex.lock();
    for(int i=0;i<coupleParameterList.size();i++)
    {
        if(followAxisId==coupleParameterList[i].followAxisId)
        {
            coupleParaOut=coupleParameterList[i];
            //usingMutex.unlock();
            return 1;
        }

    }
    //usingMutex.unlock();
    return -1;
}

bool CoupleControlManagement::isInitialOk()
{
    return isInitialOkStatus;
}

bool CoupleControlManagement::isUseThisModule()
{
    return isUseCoupleControl;
}

int CoupleControlManagement::loopRealtimeOnce()
{
    if(false==isUseCoupleControl)
    {
        return 0;
    }

    //usingMutex.lock();
    for(int i=0;i<coupleParameterList.size();i++)
    {
        if(coupleParameterList[i].isValid)
        {
            calculateOneCouple(coupleParameterList[i],axisGroup);
        }
        if(coupleParameterList[i].isMonitorRelativeDistance)
        {
            monitorRelativeDistance_OneCouple(coupleParameterList[i],axisGroup);
        }

    }
    //usingMutex.unlock();
    return 1;
}

int CoupleControlManagement::disableCouple()
{
    //usingMutex.lock();
    for(int i=0;i<coupleParameterList.size();i++)
    {
         coupleParameterList[i].isValid=false;

    }
    //usingMutex.unlock();
    return 1;
}

int CoupleControlManagement::setCoupleControlEnable(int followAxisId, bool isEnableIn)
{
//    qDebug()<<"CoupleControlManagement::setCoupleControlEnable,followAxisId"<<followAxisId<<"isEnableIn"<<isEnableIn;
    //usingMutex.lock();
    for(int i=0;i<coupleParameterList.size();i++)
    {
        if(followAxisId==coupleParameterList[i].followAxisId)
        {
            resetCoupleOffset(coupleParameterList[i],axisGroup);
            coupleParameterList[i].isValid=isEnableIn;
            //usingMutex.unlock();
//            qDebug()<<" setCoupleControlEnable sucess";
            return 1;
        }

    }
    //usingMutex.unlock();
//    qDebug()<<" setCoupleControlEnable failed, no exist couple axis";
    return -1;
}

int CoupleControlManagement::setCoupleControlOffset(int followAxisId, double offfsetValueIn)
{

    //usingMutex.lock();
//    for(int i=0;i<coupleParameterList.size();i++)
//    {
//        if(coupleId==coupleParameterList[i].followAxisId)
//        {
//            coupleParameterList[i].followOffsetValue=offfsetValueIn;
//            return 1;
//        }

//    }
    //usingMutex.unlock();
    return -1;
}

int CoupleControlManagement::setAgvAngleInMap(double angleIn)
{
    agvAngle_map=angleIn;
    return 1;
}

int CoupleControlManagement::resetCoupleOffset()
{
//    qDebug()<<"CoupleControlManagement::resetCoupleOffset()";
    if(false==isUseCoupleControl)
    {
        return 0;
    }

    filtering_mapFollowAxisPos->resetAll();

    //usingMutex.lock();
    for(int i=0;i<coupleParameterList.size();i++)
    {
//        if(coupleParameterList[i].isValid)
//        {
            resetCoupleOffset(coupleParameterList[i],axisGroup);
//        }

    }
    //usingMutex.unlock();
    return 1;
}

int CoupleControlManagement::disableAllCouple()
{
//    qDebug()<<"CoupleControlManagement::disableAllCouple";
    //usingMutex.lock();
    for(int i=0;i<coupleParameterList.size();i++)
    {
         coupleParameterList[i].isValid=false;
    }
    //usingMutex.unlock();
//    qDebug()<<" setCoupleControlEnable failed, no exist couple axis";
    return 1;
}

int CoupleControlManagement::translatePN180(double &degree)
{
    int turns=degree/360;
    degree=degree-turns*360;
    if(degree>180.0)
    {
        degree-=360;
    }
    else if(degree<-180.0)
    {
        degree+=360;
    }
    else
    {
    }
    return 1;
}


int CoupleControlManagement::resetCoupleOffset(CoupleConfigParameter &couplePara, AxisGroup *axisGroupIn)
{
    QVector<double> jointsPositionOut, jointsVelOut,jointsAccOut, jointsTorqueOut;
    axisGroupIn->getJointFeedback(jointsPositionOut, jointsVelOut,jointsAccOut, jointsTorqueOut);
    if(E_COUPLE_TYPE_MAP_R==couplePara.coupleType)
    {
//        double absoluteIdealAngle;
//        getBestPositionFromAngle(agvAngle_map,jointsPositionOut[couplePara.followAxisId],absoluteIdealAngle);
        // followValue=absoluteIdealAngle*couplePara.coupleRatio+couplePara.followOffsetValue;
        couplePara.followOffsetValue=jointsPositionOut[couplePara.followAxisId]
                                        -agvAngle_map*couplePara.coupleRatio;
//        qDebug()<<"couplePara.followOffsetValue"<<couplePara.followOffsetValue
//                  <<"jointsPositionOut[couplePara.followAxisId]"<<jointsPositionOut[couplePara.followAxisId]
//                    <<"couplePara.coupleRatio"<<couplePara.coupleRatio;

    }
    else
    {
        couplePara.followOffsetValue=jointsPositionOut[couplePara.followAxisId]
                                        -jointsPositionOut[couplePara.driveAxisId]*couplePara.coupleRatio;
    }

    return 1;
}

int CoupleControlManagement::getBestPositionFromAngle(double idealAngle, double initialPosition, double &bestPositionOut)
{
    int currentTurns;
    if(0<initialPosition)
    {
        currentTurns=(initialPosition+180) /(360);
    }
    else
    {
        currentTurns=(initialPosition-180) /(360);
    }

    double angle1,angle2,angle3;
    angle1=currentTurns*360+idealAngle;
    angle2=angle1+360;
    angle3=angle1-360;
    double offset1,offset2,offset3;
    double minNum;
    int minIndex;
    offset1=fabs(angle1-initialPosition);
    offset2=fabs(angle2-initialPosition);
    offset3=fabs(angle3-initialPosition);
    if(offset1<=offset2)
    {
        minNum=offset1;
        minIndex=1;
    }
    else
    {
        minNum=offset2;
        minIndex=2;
    }

    if(offset3<=minNum)
    {
        minNum=offset3;
        minIndex=3;
    }
    switch(minIndex)
    {
    case 1:
    {
        bestPositionOut=angle1;
        break;

    }
    case 2:
    {
        bestPositionOut=angle2;
        break;

    }
    default:
    {
        bestPositionOut=angle3;
    }
    }


    return 1;
}

int CoupleControlManagement::calculateOneCouple(const CoupleConfigParameter &couplePara, AxisGroup *axisGroupIn)
{
    QVector<double> jointsPositionOut;
    axisGroupIn->getJointCommand(jointsPositionOut);
    if(jointsPositionOut.size()<=couplePara.driveAxisId || 0>couplePara.driveAxisId)
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoupleControlManagement", 22152, robotId,couplePara.driveAxisId+1);
        return -1;
    }
    if(jointsPositionOut.size()<=couplePara.followAxisId || 0>couplePara.followAxisId)
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoupleControlManagement", 22153, robotId,couplePara.followAxisId+1);
        return -1;
    }
    double sourceValue=jointsPositionOut[couplePara.driveAxisId];
    double followValue=jointsPositionOut[couplePara.followAxisId];
    switch(couplePara.coupleType)
    {
    case E_COUPLE_TYPE_COMMON:
    {
        followValue=sourceValue*couplePara.coupleRatio+couplePara.followOffsetValue;
        //不判断 couplePara.allowMaxFollowOffset,axisObject里面做跟随误差判断.

        break;
    }
    case E_COUPLE_TYPE_MAP_R://处理多圈问题
    {


        double followValue_ok=getMinAngleReturn_new(followValue,couplePara);

        if(fabs(followValue_ok-followValue)> couplePara.allowMaxFollowOffset)
        {
            QString tmpInfo=QString::number(followValue_ok)+"  ,"+QString::number(followValue)
                    +"  ,"+QString::number(couplePara.followOffsetValue);
            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoupleControlManagement", 22154, robotId,couplePara.followAxisId+1,
                   0,followValue_ok-followValue,couplePara.allowMaxFollowOffset,tmpInfo.toStdString());

            qDebug() <<"followValue_ok"<<followValue_ok
                        <<"followValue"<<followValue
                        <<"couplePara.followOffsetValue"<<couplePara.followOffsetValue;
            return -1;
        }

//        followValue=filtering_mapFollowAxisPos->medianMean(followValue_ok,10);
        followValue=followValue_ok;

        break;
    }
    default:
    {
        return 0;
    }
    }
    axisGroupIn->moveOneStepByPosition(followValue,couplePara.followAxisId);
    return 1;
}

int CoupleControlManagement::monitorRelativeDistance_OneCouple(CoupleConfigParameter &couplePara,
                                                               AxisGroup *axisGroupIn)
{
    QVector<double> jointsPositionOut;
    axisGroupIn->getJointCommand(jointsPositionOut);
    if(jointsPositionOut.size()<=couplePara.driveAxisId || 0>couplePara.driveAxisId)
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoupleControlManagement", 22152, robotId,couplePara.driveAxisId+1);
        return -1;
    }
    if(jointsPositionOut.size()<=couplePara.followAxisId || 0>couplePara.followAxisId)
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoupleControlManagement", 22153, robotId,couplePara.followAxisId+1);
        return -2;
    }
    double sourceValue=jointsPositionOut[couplePara.driveAxisId];
    double followValue=jointsPositionOut[couplePara.followAxisId];
    switch(couplePara.coupleType)
    {
    case E_COUPLE_TYPE_COMMON:
    {
        double tmpRelativeDistance=followValue-sourceValue;


        if(tmpRelativeDistance> couplePara.allowMaxRelativeDistance)
        {
            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoupleControlManagement", 22155, robotId,
                   couplePara.followAxisId+1,
                   couplePara.driveAxisId+1,tmpRelativeDistance,couplePara.allowMaxRelativeDistance);

            return -1;
        }
        else if(tmpRelativeDistance< couplePara.allowMinRelativeDistance)
        {
            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoupleControlManagement", 22156, robotId,
                   couplePara.followAxisId+1,
                   couplePara.driveAxisId+1,tmpRelativeDistance,couplePara.allowMinRelativeDistance);

            return -1;
        }
        //减速判断
        if(tmpRelativeDistance> couplePara.allowMaxRelativeDistance-couplePara.approachDistance
                && tmpRelativeDistance>couplePara.currentRelativeDistance)
        {
            couplePara.isNeedCease=true;
            couplePara.currentRelativeDistance=tmpRelativeDistance;
            addMsg(ENUM_MSG_WARNING, "McControllerInterpreter", "CoupleControlManagement", 22157, robotId,
                   couplePara.followAxisId+1,
                   couplePara.driveAxisId+1,tmpRelativeDistance,couplePara.allowMaxRelativeDistance);

            return -1;
        }
        else if(tmpRelativeDistance< couplePara.allowMinRelativeDistance+couplePara.approachDistance
                && tmpRelativeDistance<couplePara.currentRelativeDistance)
        {
            couplePara.isNeedCease=true;
            couplePara.currentRelativeDistance=tmpRelativeDistance;
            addMsg(ENUM_MSG_WARNING, "McControllerInterpreter", "CoupleControlManagement", 22158, robotId,
                   couplePara.followAxisId+1,
                   couplePara.driveAxisId+1,tmpRelativeDistance,couplePara.allowMinRelativeDistance);

            return -1;
        }
        couplePara.isNeedCease=false;
        couplePara.currentRelativeDistance=tmpRelativeDistance;

        break;
    }
    case E_COUPLE_TYPE_MAP_R:
    {



        break;
    }
    default:
    {
        return 0;
    }
    }

    return 1;
}


#define D_PARAMETER_COUPLE_NODE_COUNT 11

int CoupleControlManagement::initialConfigFile(QString configFileName)
{
    qDebug() << "CoupleControlManagement::initialConfigFile ==== 初始化配置文件：" << configFileName;

    coupleParameterList.clear();


    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << " 配置文件打开失败：" << configFileName;
        }
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "isUseCoupleControl", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, isUseCoupleControl);
    if(false==isUseCoupleControl)
    {
        qDebug() << "not use CoupleControlManagement!";
        return 1;
    }

    ///beltParameter读参数
    QDomNode  inNode = domparser.findSubNode(domparser.getRootDomElement(), "coupleParameter", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"读文件失败: "<<configFileName;
        }
        return -1;
    }

    int count;

   domparser. readXml( domparser.findSubNode(inNode, "count", ok), count  );
    if(!ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"读文件失败: ";
        }
        return -1;
    }


    QVector<bool> okReturn;
    okReturn.resize(D_PARAMETER_COUPLE_NODE_COUNT);
    for(int i=0;i<count;i++)
    {
         QDomNode  tmpNode =  domparser.findSubNode(inNode, QString("couple%1").arg(i), ok);
         if( !ok  )
         {
            if(IF_DEBUG)
            {
                qDebug()<<"读内部参数失败:  "<<i;
            }
            return -1;
         }
        CoupleConfigParameter tmpConfig;
        tmpConfig.isNeedCease=false;
        //节点读取
        bool tmpValid;
        domparser.readXml( domparser.findSubNode(tmpNode, "isValid", okReturn[0]),  tmpValid );
        tmpConfig.isValid =tmpValid;
        int tmpType;
        domparser.readXml( domparser.findSubNode(tmpNode, "coupleType", okReturn[1]), tmpType );
        tmpConfig.coupleType =(E_COUPLE_TYPE)tmpType;
        domparser.readXml( domparser.findSubNode(tmpNode, "driveAxisId", okReturn[2]),  tmpConfig.driveAxisId  );
        domparser.readXml( domparser.findSubNode(tmpNode, "followAxisId", okReturn[3]),  tmpConfig.followAxisId  );
        domparser.readXml( domparser.findSubNode(tmpNode, "coupleRatio", okReturn[4]),  tmpConfig.coupleRatio  );
        domparser.readXml( domparser.findSubNode(tmpNode, "allowMaxFollowOffset", okReturn[5]),  tmpConfig.allowMaxFollowOffset  );
        domparser.readXml( domparser.findSubNode(tmpNode, "isMonitorRelativeDistance", okReturn[6]),  tmpConfig.isMonitorRelativeDistance  );
        domparser.readXml( domparser.findSubNode(tmpNode, "allowMinRelativeDistance", okReturn[7]),  tmpConfig.allowMinRelativeDistance  );
        domparser.readXml( domparser.findSubNode(tmpNode, "allowMaxRelativeDistance", okReturn[8]),  tmpConfig.allowMaxRelativeDistance  );
                domparser.readXml( domparser.findSubNode(tmpNode, "approachDistance", okReturn[9]),  tmpConfig.approachDistance  );
        double tmpFollow;
        domparser.readXml( domparser.findSubNode(tmpNode, "followOffsetValue", okReturn[D_PARAMETER_COUPLE_NODE_COUNT-1]),
                tmpFollow);
        tmpConfig.followOffsetValue =tmpFollow;
        tmpConfig.isValid=false;
        for(int k=0;k<okReturn.size();k++)
        {
           if( !okReturn[k]  )
           {
              if(IF_DEBUG)
              {
                  qDebug()<<"ZeroManagement::readFile()====读内部轴参数节点失败: axis "<<i<<"axisParameter"<<k;
              }
              return -1;
           }
        }
        coupleParameterList.append(tmpConfig);
    }



    domparser.closeXml();
    return 1;
}

#define D_MAX_ANGLE_COMPARE_COUNT 17  //是个ｂｕｇ ,聿冠测试，需要改进。超过圈数后可能就不对了。
double CoupleControlManagement::getMinAngleReturn_new(double followValue,const CoupleConfigParameter &couplePara)
{
//    couplePara.followOffsetValue=jointsPositionOut[couplePara.followAxisId]
//                                    -agvAngle_map*couplePara.coupleRatio;
    double tmpagvAngle_map=(followValue-couplePara.followOffsetValue)/couplePara.coupleRatio;

    int currentTurns=tmpagvAngle_map/360;

    QVector<double>  angle;
    angle.resize(D_MAX_ANGLE_COMPARE_COUNT);
    for(int i=0;i<angle.size();i++)
    {
        angle[i]=currentTurns*360+agvAngle_map-360*(D_MAX_ANGLE_COMPARE_COUNT-1)/2+i*360;
    }
//    angle1=currentTurns*360+agvAngle_map;
//    angle2=currentTurns*360+agvAngle_map+360;
//    angle3=currentTurns*360+agvAngle_map-360;
//    angle4=currentTurns*360+agvAngle_map+720;
//    angle5=currentTurns*360+agvAngle_map-720;

    QVector<double> followValue_new;
    for(int i=0;i<angle.size();i++)
    {
        followValue_new.append(angle[i]*couplePara.coupleRatio+couplePara.followOffsetValue);
    }
//    followValue_new.append(angle1*couplePara.coupleRatio+couplePara.followOffsetValue);
//    followValue_new.append(angle2*couplePara.coupleRatio+couplePara.followOffsetValue);
//    followValue_new.append(angle3*couplePara.coupleRatio+couplePara.followOffsetValue);
//    followValue_new.append(angle4*couplePara.coupleRatio+couplePara.followOffsetValue);
//    followValue_new.append(angle5*couplePara.coupleRatio+couplePara.followOffsetValue);

    QVector<double> angleList;
    for(int i=0;i<followValue_new.size();i++)
    {
        angleList.append(followValue_new[i]-followValue);
    }


    int tmpIndex=0;
    double tmpMin=angleList[0];
    for(int i=1;i<angleList.size();i++)
    {
        if(fabs(angleList[i]) < tmpMin)
        {
            tmpIndex=i;
            tmpMin=angleList[i];

        }
    }

    return followValue_new[tmpIndex];

}


double CoupleControlManagement::getMinAngleReturn(double followValue,const CoupleConfigParameter &couplePara)
{
    int currentTurns=followValue/360;

    double angle1,angle2,angle3;
    angle1=currentTurns*360+agvAngle_map;
    angle2=agvAngle_map+360;
    angle3=agvAngle_map-360;

    double followValue_new1=angle1*couplePara.coupleRatio+couplePara.followOffsetValue;
    double followValue_new2=angle2*couplePara.coupleRatio+couplePara.followOffsetValue;
    double followValue_new3=angle3*couplePara.coupleRatio+couplePara.followOffsetValue;

    double angleMove1=followValue_new1-followValue;
    double angleMove2=followValue_new2-followValue;
    double angleMove3=followValue_new3-followValue;

    double followValue_ok;

    if(fabs(angleMove1)<fabs(angleMove2))
    {
        if(fabs(angleMove1)<fabs(angleMove3))
        {
            followValue_ok=followValue_new1;
        }
        else
        {
            followValue_ok=followValue_new3;
        }
    }
    else
    {
        if(fabs(angleMove2)<fabs(angleMove3))
        {
            followValue_ok=followValue_new2;
        }
        else
        {
            followValue_ok=followValue_new3;
        }
    }
}

int CoupleControlManagement::checkFollowOffset(double oldPos,double newPos)
{
    //当前偏差合理性判断
//    double absoluteIdealAngle;
//    getBestPositionFromAngle(agvAngle_map,followValue,absoluteIdealAngle);
////        if(fabs(absoluteIdealAngle-followValue+couplePara.followOffsetValue)>
////                                                        couplePara.allowMaxFollowOffset)
//    double tmpError=followValue-absoluteIdealAngle*couplePara.coupleRatio
//                         -couplePara.followOffsetValue;
//    //转换【－180,180]
//    translatePN180(tmpError);
//    if(fabs(tmpError)> couplePara.allowMaxFollowOffset)
//    {
//        QString tmpInfo=QString::number(followValue)+"  ,"+ QString::number(absoluteIdealAngle*couplePara.coupleRatio)+"  ,"
//                +QString::number(couplePara.followOffsetValue);
//        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoupleControlManagement", 22154, robotId,couplePara.followAxisId,
//               0,tmpError,couplePara.allowMaxFollowOffset,tmpInfo.toStdString());

//        qDebug() <<"followValue"<<followValue
//                    <<"absoluteIdealAngle*couplePara.coupleRatio"<<absoluteIdealAngle*couplePara.coupleRatio
//                    <<"couplePara.followOffsetValue"<<couplePara.followOffsetValue
//                      <<"tmpError"<<tmpError;
//        return -1;
//    }
}

void CoupleControlManagement::addMsg(int messageLevel, std::string componentName, std::string messageType
                            , int messageCode, int robotIdIn, int parameter1, int parameter2,
                                     double parameter3,double parameter4,std::string messageInfo)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;
    QString tmpStr;

    switch(messageCode)
    {
    case 22158:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",耦合相对距离监视警告：跟随轴(%1)相对主动轴(%2)的相对位置(%3)接近负极限(%4)").arg(parameter1)
                .arg(parameter2).arg(parameter3).arg(parameter4);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22157:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",耦合相对距离监视警告：跟随轴(%1)相对主动轴(%2)的相对位置(%3)接近正极限(%4)").arg(parameter1)
                .arg(parameter2).arg(parameter3).arg(parameter4);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22156:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",耦合相对距离监视触发：跟随轴(%1)相对主动轴(%2)的相对位置(%3)超过负极限(%4)").arg(parameter1)
                .arg(parameter2).arg(parameter3).arg(parameter4);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22155:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",耦合相对距离监视触发：跟随轴(%1)相对主动轴(%2)的相对位置(%3)超过正极限(%4)").arg(parameter1)
                .arg(parameter2).arg(parameter3).arg(parameter4);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22154:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",耦合跟随轴(%1)跟随失败，跟随误差(%2)超过允许值(%3)").arg(parameter1)
                .arg(parameter3).arg(parameter4)+QString::fromStdString(messageInfo);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22153:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",耦合跟随轴(%1)超过有效范围").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22152:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",耦合驱动轴(%1)超过有效范围").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22151:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",耦合序号(%1)不存在，设置耦合偏差失败").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22150:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",耦合配置文件读取失败");
        infomationStr = tmpStr.toStdString();
        break;
    }



        default:
        {
            qDebug()<<"CoupleControlManagement::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}

